{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PreintegrationParams\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegrationParams.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `PreintegrationParams` class holds the parameters required for Inertial Measurement Unit (IMU) preintegration in GTSAM. It configures how IMU measurements (accelerometer and gyroscope) are handled, including noise characteristics, gravity direction, and Coriolis effects.\n",
    "\n",
    "This class extends `PreintegratedRotationParams` (which handles gyroscope-specific parameters) by adding parameters relevant to accelerometer measurements and the combined integration process.\n",
    "\n",
    "A single `PreintegrationParams` object (usually created as a `shared_ptr`) is typically shared among multiple preintegration instances (`PreintegratedImuMeasurements` or `PreintegratedCombinedMeasurements`)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Parameters\n",
    "\n",
    "In addition to parameters inherited from `PreintegratedRotationParams` (like `gyroscopeCovariance`, `omegaCoriolis`, `body_P_sensor`), `PreintegrationParams` includes:\n",
    "\n",
    "- **`accelerometerCovariance`**: A 3x3 matrix representing the continuous-time noise covariance of the accelerometer. Units: (m/s²)²/Hz.\n",
    "- **`integrationCovariance`**: A 3x3 matrix representing additional uncertainty introduced during the numerical integration process itself (often set to zero). Units: (m²/s⁶)/Hz ? (Consult documentation for precise interpretation).\n",
    "- **`use2ndOrderCoriolis`**: A boolean flag. If true, enables a more accurate (second-order) correction for Coriolis effects, which arise from the Earth's rotation.\n",
    "- **`n_gravity`**: A 3D vector specifying the direction and magnitude of gravity in the navigation frame (e.g., [0, 0, -9.81] for ENU, [0, 0, 9.81] for NED). Units: m/s²."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Constructors**: \n",
    "  - `PreintegrationParams(n_gravity)`: Main constructor requiring the gravity vector.\n",
    "  - `MakeSharedD(g=9.81)`: Convenience static method to create shared parameters for a Z-down (NED) navigation frame.\n",
    "  - `MakeSharedU(g=9.81)`: Convenience static method to create shared parameters for a Z-up (ENU) navigation frame.\n",
    "- **Setters**: Methods like `setAccelerometerCovariance`, `setIntegrationCovariance`, `setUse2ndOrderCoriolis`, `setGyroscopeCovariance`, `setOmegaCoriolis`, `setBodyPSensor` allow configuring the parameters after creation.\n",
    "- **Getters**: Corresponding methods like `getAccelerometerCovariance`, `getIntegrationCovariance`, `getGravity`, `isUsing2ndOrderCoriolis` allow retrieving parameter values.\n",
    "- **`print` / `equals`**: Standard methods for displaying parameters and comparing them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Typically, you create parameters once using a convenience function and then adjust specific noise values."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "IMU Preintegration Parameters:\n",
      "\n",
      "gyroscopeCovariance:\n",
      "[\n",
      "1e-06     0     0\n",
      "    0 1e-06     0\n",
      "    0     0 1e-06\n",
      "]\n",
      "accelerometerCovariance:\n",
      "[\n",
      "0.0001      0      0\n",
      "     0 0.0001      0\n",
      "     0      0 0.0001\n",
      "]\n",
      "integrationCovariance:\n",
      "[\n",
      "1e-08     0     0\n",
      "    0 1e-08     0\n",
      "    0     0 1e-08\n",
      "]\n",
      "n_gravity = (    0     0 -9.81)\n"
     ]
    }
   ],
   "source": [
    "from gtsam import PreintegrationParams\n",
    "import numpy as np\n",
    "\n",
    "# Create parameters for an ENU navigation frame (Z-up)\n",
    "# Using standard gravity, g=9.81 m/s^2\n",
    "params = PreintegrationParams.MakeSharedU(9.81)\n",
    "\n",
    "# Set accelerometer noise characteristics (example values)\n",
    "accel_noise_sigma = 0.01  # m/s^2 / sqrt(Hz)\n",
    "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
    "\n",
    "# Set gyroscope noise characteristics (example values)\n",
    "gyro_noise_sigma = 0.001 # rad/s / sqrt(Hz)\n",
    "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
    "\n",
    "# Set integration uncertainty (often zero)\n",
    "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
    "\n",
    "# Optionally, set sensor pose relative to body\n",
    "# params.setBodyPSensor(gtsam.Pose3(...))\n",
    "\n",
    "# Optionally, enable 2nd order Coriolis correction\n",
    "# params.setUse2ndOrderCoriolis(True)\n",
    "\n",
    "print(\"IMU Preintegration Parameters:\")\n",
    "params.print()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These parameters are then passed to `PreintegratedImuMeasurements` or `PreintegratedCombinedMeasurements` when they are constructed."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [PreintegrationParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.h)\n",
    "- [PreintegrationParams.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.cpp)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
